from ctypes import *
import time
import os
import threading
from ctypes import c_int32
from utils.log_manager import logger  # 日志模块
from PySide6.QtCore import QObject, QCoreApplication

class MotorController(QObject):
    def __init__(self, port='USB', step_angle=1.8, subdivision=32, screw_pitch=0.5, transmission_ratio=1.0):
        # 初始化控制器，设置电机物理参数。
        # step_angle: 步进角度（例如1.8°）
        # subdivision: 每转的脉冲数（细分数）
        # screw_pitch: 丝杆螺距（单位mm）
        # transmission_ratio: 传动比（通常为1）
        super().__init__()
        self.port = port
        self.step_angle = step_angle
        self.subdivision = subdivision
        self.screw_pitch = screw_pitch
        self.transmission_ratio = transmission_ratio
        self.max_range_mm = 25.0  # 最大运动范围（mm）

        dll_name = "MT_API.dll"
        dll_path = os.path.join(os.path.dirname(__file__), dll_name)

        if not os.path.exists(dll_path):
            raise FileNotFoundError(QCoreApplication.translate("MotorController","❌ 未找到 {dll_name}，请将它放在 drivers/ 目录下：{dll_path}").format(dll_name=dll_name, dll_path=dll_path))

        self.api = windll.LoadLibrary(dll_path)
        self.api.MT_Init()
        logger.log(QCoreApplication.translate("MotorController","初始化 MT_API.dll 成功"))

        # 设置单位转换函数类型
        self.api.MT_Help_Step_Line_Steps_To_Real.argtypes = [c_double, c_int32, c_double, c_double, c_int32]
        self.api.MT_Help_Step_Line_Steps_To_Real.restype = c_double
        self.api.MT_Help_Step_Line_Real_To_Steps.argtypes = [c_double, c_int32, c_double, c_double, c_double]
        self.api.MT_Help_Step_Line_Real_To_Steps.restype = c_int32

        self.connected = False
        self.positions = {0: 0.0, 1: 0.0}  # 用于内部记录当前坐标（避免频繁调用外设）
    
    def is_connected(self):
        """检查控制器是否实际连接"""
        try:
            return self.api.MT_Check() == 0
        except Exception:
            return False

    def connect(self):
        """连接控制器（USB）"""
        if self.is_connected():
            logger.log(QCoreApplication.translate("MotorController","⚠️ 控制器已连接，跳过重复连接"))
            return True

        self.api.MT_Open_USB()
        success = self.api.MT_Check() == 0
        self.connected = success
        logger.log(QCoreApplication.translate("MotorController","控制器连接状态: ") + (QCoreApplication.translate("MotorController","成功") if success else QCoreApplication.translate("MotorController","失败")))
        return success

    def disconnect(self):
        """断开连接并释放资源"""
        if not self.is_connected():
            logger.log(QCoreApplication.translate("MotorController","⚠️ 控制器未连接，无需断开"))
            return

        self.api.MT_Close_USB()
        self.api.MT_DeInit()
        self.connected = False
        logger.log(QCoreApplication.translate("MotorController","已断开控制器连接"))

    def set_speed(self, axis, acc_mm_s2, dec_mm_s2, vmax_mm_s):
        # 设置速度参数（单位为 mm/s² 和 mm/s）
        # 会调用 mm_to_steps() 进行换算
        acc = int(self.mm_to_steps(acc_mm_s2))    # mm/s² → pulse/s²
        dec = int(self.mm_to_steps(dec_mm_s2))    # mm/s² → pulse/s²
        vmax = int(self.mm_to_steps(vmax_mm_s))   # mm/s → pulse/s
        vmax = min(vmax,  int(self.mm_to_steps(2.5)))

        self.api.MT_Set_Axis_Mode_Position(c_int32(axis))
        self.api.MT_Set_Axis_Position_Acc(c_int32(axis), c_int32(acc))
        self.api.MT_Set_Axis_Position_Dec(c_int32(axis), c_int32(dec))
        self.api.MT_Set_Axis_Position_V_Max(c_int32(axis), c_int32(vmax))
        logger.log(QCoreApplication.translate("MotorController","[{axis}] 设置速度 acc={acc_mm_s2} dec={dec_mm_s2} vmax={vmax_mm_s} mm/s").format(axis=axis, acc_mm_s2=acc_mm_s2, dec_mm_s2=dec_mm_s2, vmax_mm_s=vmax_mm_s))

    def move_to(self, axis, target_mm):
        """移动到目标位置（绝对位置，单位mm）"""
        # if abs(target_mm) > self.max_range_mm:
        #     raise ValueError("超出允许移动范围 ±25mm")
        steps = self.mm_to_steps(target_mm)
        self.api.MT_Set_Axis_Position_P_Target_Abs(axis, steps)
        logger.log(QCoreApplication.translate("MotorController","[{axis}] 绝对移动到 {target_mm:.3f} mm -> {steps} 步").format(axis=axis, target_mm=target_mm, steps=steps))
        self.positions[axis] = target_mm

    def move_by(self, axis, offset_mm):
        """相对当前位置移动 offset（单位mm）"""
        steps = self.mm_to_steps(offset_mm)
        self.api.MT_Set_Axis_Position_P_Target_Rel(axis, steps)
        logger.log(QCoreApplication.translate("MotorController","[{axis}] 相对移动 {offset_mm:.3f} mm -> {steps} 步").format(axis=axis, offset_mm=offset_mm, steps=steps))
        self.positions[axis] += offset_mm

    def stop(self, axis):
        """正常停止（带减速）"""
        self.api.MT_Set_Axis_Position_Stop(axis)
        logger.log(QCoreApplication.translate("MotorController","[{axis}] 停止").format(axis=axis))

    def halt_all(self):
        """急停所有轴"""
        self.api.MT_Set_Axis_Halt_All()
        logger.log(QCoreApplication.translate("MotorController","急停所有轴"))

    def get_position(self, axis):
        """获取当前位置（mm）"""
        iPos = c_int32(0)
        self.api.MT_Get_Axis_Software_P_Now(axis, pointer(iPos))
        mm = self.steps_to_mm(iPos.value)
        self.positions[axis] = mm
        return round(mm, 3)

    def get_position_xy(self):
        """获取两个轴的当前位置"""
        return self.get_position(0), self.get_position(1)

    def set_position(self, axis, value_mm):
        """设置当前为指定位置"""
        steps = self.mm_to_steps(value_mm)
        self.api.MT_Set_Axis_Software_P(axis, steps)
        self.positions[axis] = value_mm
        logger.log(QCoreApplication.translate("MotorController","[{axis}] 设置当前位置为 {value_mm} mm ({steps} 步)").format(axis=axis, value_mm=value_mm, steps=steps))

    def is_moving(self, axis):
        """判断是否正在运动"""
        iRun = c_int32(0)
        self.api.MT_Get_Axis_Status_Run(axis, pointer(iRun))
        return iRun.value != 0

    def is_any_axis_moving(self):
        """是否任意轴在运动中"""
        return self.is_moving(0) or self.is_moving(1)
    
    def move_xy(self, x_mm, y_mm):
        """同时移动 X/Y 两轴（单位 mm）"""
        self.move_to(0, x_mm)
        self.move_to(1, y_mm)
        logger.log(QCoreApplication.translate("MotorController","[XY] 同时移动到 X={x_mm:.3f} mm, Y={y_mm:.3f} mm").format(x_mm=x_mm, y_mm=y_mm))


    def wait_for_motion_done(self, axis, timeout=60):
        """等待运动完成，超时返回False"""
        start = time.time()
        while self.is_moving(axis):
            if time.time() - start > timeout:
                logger.log(QCoreApplication.translate("MotorController","[{axis}] 等待超时").format(axis=axis))
                return False
            time.sleep(0.05)
        return True

    def home_axis(self, axis):
        # 自动归零：打负限位 -> 记录 -> 打正限位 -> 记录 -> 中点 -> 设置为0
        # 参数单位：mm/s²、mm/s，归零过程在子线程中运行
        def _do_home():
            logger.log(QCoreApplication.translate("MotorController","[{axis}] 自动归零开始").format(axis=axis))

            self.move_by(axis, -30)
            self.wait_for_motion_done(axis, timeout=30)
            p_neg = self.get_position(axis)
            time.sleep(0.5)

            self.move_by(axis, 30)
            self.wait_for_motion_done(axis, timeout=30)
            p_pos = self.get_position(axis)
            time.sleep(0.5)

            mid = round((p_neg + p_pos) / 2.0, 3)
            self.move_to(axis, mid)
            self.wait_for_motion_done(axis, timeout=30)
            self.set_position(axis, 0)

            logger.log(QCoreApplication.translate("MotorController","[{axis}] 归零完成 负限: {p_neg:.3f} 正限: {p_pos:.3f} 中心: {mid:.3f}").format(axis=axis, p_neg=p_neg, p_pos=p_pos, mid=mid))

        threading.Thread(target=_do_home, daemon=True).start()

    def mm_to_steps(self, mm):
        """mm 转脉冲"""
        return self.api.MT_Help_Step_Line_Real_To_Steps(
            self.step_angle, self.subdivision, self.screw_pitch, self.transmission_ratio, mm)

    def steps_to_mm(self, steps):
        """脉冲转 mm"""
        return self.api.MT_Help_Step_Line_Steps_To_Real(
            self.step_angle, self.subdivision, self.screw_pitch, self.transmission_ratio, steps)

    def debug_print_conversion(self):
        """调试换算系数"""
        print(f"1 mm = {self.mm_to_steps(1)} 步")
        print(f"0.1 mm = {self.mm_to_steps(0.1)} 步")
        print(f"6400 步 = {self.steps_to_mm(6400):.4f} mm")